Method of calibrating a camera and a system therefor

ABSTRACT

A system and method for calibrating a camera includes an energy source and a camera to be calibrated, with at least one of the energy source and the camera being mounted on a mechanical actuator so that it is movable relative to the other. A processor is connected to the energy source, the mechanical actuator and the camera and is programmed to control the mechanical actuator to move at least one of the energy source and the camera relative to the other through a plurality of discrete points on a calibration target pattern. The processor further, at each of the discrete points, controls the camera to take a digital image and perform a lens distortion characterisation on each image. A focal length of the camera is determined including any lens connected to the camera and an extrinsic camera position for each image is then determined.

BACKGROUND OF THE INVENTION

The present application relates to a method of calibrating a camera anda system therefor.

The methodology firstly characterizes the lens distortion and thendetermines the focal length and finally determines the extrinsicpositions of the cameras.

Using the above methodology, the present invention provides an improvedmethod of calibrating a camera and a system therefor.

SUMMARY OF THE INVENTION

According to one example embodiment, a system for calibrating a camera,the system including:

-   -   an energy source and a camera to be calibrated, with at least        one of the energy source and the camera being mounted on a        mechanical actuator so that it is movable relative to the other;    -   a processor connected to the energy source, the mechanical        actuator and the camera, the processor programmed to:    -   control the mechanical actuator to move at least one of the        energy source and the camera relative to the other through a        plurality of discrete points on a calibration target pattern;    -   at each of the discrete points, control the camera to take a        digital image;    -   perform a lens distortion characterisation on each image;    -   determine a focal length of the camera including any lens        connected to the camera; and    -   determine an extrinsic camera position for each image.

The processor may perform the lens distortion characterisation by:

-   -   choosing a distortion correction model and determine an initial        estimate of parameters for this model to correct an observed        distortion;    -   choosing a line straightness metric, which measures and        quantifies co-linear points along a sampled line; and    -   using the line straightness metric and numerically refining the        initial estimated parameters until the lines in the distortion        corrected image.

The processor may determine a focal length by:

-   -   selecting an initial focal length;    -   using algorithms in combination with the initial focal length,        physical pixel sizes, undistorted image coordinates of the        energy source at each point in the sequence, and the exact        positions of the mechanical actuator at each point in the        sequence to determine the position of the camera relative to        each discrete point;    -   determine how tightly clustered the camera positions are; and    -   numerically refine the initial focal length until the determined        discrete points are most tightly packed.

The processor may determine an extrinsic camera position by:

-   -   creating a bundle of geometry based vectors; creating a bundle        of image processing based vectors;    -   choosing a metric to measure the similarity of the two bundles        of vectors; and    -   refine an estimated position of the camera relative to the        energy source to maximise the similarity of the bundles of        vectors.

In one example, after the digital images have been captured, theprocessor further performs the following imaging processing steps:

-   -   determine which regions of adjacent pixels in an image have an        intensity above a chosen threshold value;    -   generate a list of such regions and the pixels which belong to        each region together with the pixels' coordinates and        intensities;    -   remove from this list any regions which have either too few or        too many constituent pixels as determined by characteristics of        the camera, lens and energy source;    -   remove from the list all regions that do not meet shape        criteria; and    -   determine a center of the largest remaining region.

The processor may determine the center by fitting an ellipse to theregion's pixel and using its center or by calculating the center ofgravity of the pixels in the region.

The shape criteria may be symmetry and wherein the symmetry is tested byfinding a cross section through the region that result in the longestprofile in terms of distance from the first pixel encountered to thelast pixel encountered and comparing this distance to that obtained whenusing the perpendicular line to the longest axis.

In one example, the processor controls the mechanical actuator to movethe mechanical actuator such that the sequence of points is divided intoseveral sets, each set containing at least 3 points in a plane and atleast one point out of the plane defined by the other points.

The precise relative displacements of these points is known by theprocessor using positional feedback from the mechanical actuator.

For example, each set is created by applying a different 6 degree offreedom translational and rotational offset to the standarduntransformed set points to yield a new set of discrete points whichhave the same relative positions.

According to another example embodiment, a method for calibrating acamera, the method including:

-   -   control a mechanical actuator to move at least one of an energy        source and a camera relative to the other through a plurality of        discrete points on a calibration target pattern;    -   at each of the discrete points, take a digital image with the        camera; perform a lens distortion characterisation on each        image;    -   determine a focal length of the camera including any lens        connected to the camera; and    -   determine an extrinsic camera position for each image.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is an example system for calibrating a digital camera; and

FIG. 2 is a block diagram illustrating a processor of the system of FIG.1.

DESCRIPTION OF EMBODIMENTS

The system and methodology described herein relate to a method ofcalibrating a camera and a system therefor.

The present invention characterises a camera of any (known) sensitivityspectrum, in terms of its intrinsic and extrinsic parameters. Theintrinsic parameters are those that affect the projection of observedreal-world scene's onto the camera's imaging electronics, these includeat least one of the lens distortion parameters, lens focal length, sizeof the pixels, and orthoganilty of the imaging electronics to the lens'optical axis.

The extrinsic parameters define at least one of where the camera isrelative to a reference point and what its orientation is relative to achosen axis definition.

Referring to the accompanying Figures, the system 10 includes at leastone camera 12 and/or at least one energy source 14.

It will be appreciated that a number of cameras 12 and a number ofenergy sources 14 could be used.

The system also includes a processor 16 connected to the energy sourceand/or the camera. The processor 16 includes a number of modules whosefunctionality will be described in more detail below.

In one example embodiment, the modules described below may beimplemented by a machine-readable medium embodying instructions which,when executed by a machine, cause the machine to perform any of themethods described above.

In another example embodiment the modules may be implemented usingfirmware programmed specifically to execute the method described herein.

It will be appreciated that embodiments of the present invention are notlimited to such architecture, and could equally well find application ina distributed, or peer-to-peer, architecture system. Thus the modulesillustrated could be located on one or more servers operated by one ormore institutions.

It will also be appreciated that in any of these cases the modules forma physical apparatus with physical modules specifically for executingthe steps of the method described herein.

A memory 28 is connected to the processor 16.

In an example embodiment, to perform the above calibrations, amechanical actuator 18 in the form of a robotic arm is used to move anenergy source in the field of view of the camera.

In the illustrated example embodiment the mechanical actuator 18 ismovable by servo motors (not shown) under the control of the processor16.

The position of the robotic arm 18 at any given time is known, and thecamera captures images of the energy source.

Each image containing a view of the robotic arm and energy source iscaptured by the camera and delivered to the processor 16. The camera 12may have been configured such that the exposure period has beendecreased and/or the iris of the lens opened up so that the energysource is visible whilst eliminating most of the background. Thealgorithms detailed below work regardless of whether this has been doneand require only a single image with the energy source visible.

The processor 16 firstly characterizes the lens distortion and thendetermines the focal length and finally determines the extrinsicpositions of the cameras. These steps will be defined in more detailbelow.

After the digital images have been captured, the image processing stepsare performed by an image processing module 20 as follows to implementthe above methodologies:

-   1) Determine which regions of adjacent pixels in the image have an    intensity above a chosen threshold value. Generate a list of such    regions and the pixels which belong to each region together with the    pixels' coordinates and intensities.-   2) Remove from this list any regions which have either too few or    too many constituent pixels as determined by the characteristics of    the camera, lens and energy source.-   3) Remove from the list all regions that do not meet shape criteria    such as symmetry. Symmetry is tested by finding which cross section    through the region results in the longest profile in terms of    distance from the first pixel encountered to the last pixel    encountered. This distance is compared to that obtained when using    the perpendicular line to the longest axis. If the ratio of the two    distances is more than a specified delta from unity the region is    discarded.-   4) The center of the largest remaining region is sought. This center    can be found by means such as fitting an ellipse to the region's    pixel and using its center, or by calculating the center of gravity    of the pixels in the region.

Lens Distortion Characterisation Steps are then implemented by a lensdistortion characterisation module 22 of the processor 16 as follows:

-   1) Rigidly place the camera in such a position that it can observe    with no obstruction the robot moving the energy source.-   2) Cause the robot to move the energy source in a series of straight    lines. While it is suggested that the lines cover the camera's    entire field of view, this is not strictly necessary.-   3) At several (at least 3) points along each line, obtain (from the    camera being characterised) and then process an image of the energy    source as described above.-   4) Choose a distortion correction model and determine the initial    estimate of the parameters for this model which will correct the    observed distortion. This work uses an augmented form of the Brown    Lens Distortion model, and uses either prior knowledge or a genetic    algorithm to yield an initial starting position.-   5) Choose a line straightness metric, which measures and quantifies    how co-linear the points along each sampled line of Step 2 are. This    work fits the least squares error best fit straight line through    each of the sampled points for each line. The metric is then the    root mean square perpendicular distance of each sampled point from    its respective best fit straight line.-   6) Using the metric of Step 5. Numerically refine the initial    estimated parameters of Step 4 until the lines in the distortion    corrected image (as produced by the distortion model and the current    set of parameters) are maximally straight. In this work    multidimensional non-linear numerical optimisation techniques are    used to minimise the metric described in Step 5. Specifically    Leapfrog and the Fletcher-Reeves Conjugate Gradient methods are    used.

After the above, the focal length determination is implemented by afocal length determination module 24 of the processor 16 as follows:

-   1) Determine the camera's lens distortion characteristics.-   2) Rigidly place the camera in such a position that it has an    unobstructed view of the robot arm moving the energy source.-   3) Move the robot such that it pauses at a sequence of discrete    points. This sequence of points is divided into several sets. Each    set contains at least 3 points in a plane and at least one point out    of the plane defined by the other points. The precise relative    displacements of these points are known using positional feedback    from the robot. Each set is created by applying a different 6 degree    of freedom translational and rotational offset to the standard    untransformed set points to yield a new set of discrete points which    have the same relative positions. For example, in a prototype    embodiment, each set of points consist of four points arranged in a    tetrahedron. At each of five locations four tetrahedrons were    created, angled up-left, up-right, down-left and down-right. This    gives a total of 20 tetrahedrons.-   4) At each of the discrete points described in step 3, process the    camera image as described above to find the coordinate of the energy    source.-   5) Use the distortion characterisation to find the undistorted pixel    position for each captured coordinate yielded by Step 4.-   6) Select an initial focal length using either prior knowledge or    the manufacturer's claimed nominal focal length.-   7) Using the algorithms described in the RANSAC paper (Martin A.    Fischler and Robert C. Bolles. 1981. Random sample consensus: a    paradigm for model fitting with applications to image analysis and    automated cartography. Commun. ACM 24, 6 (June 1981), 381-395.    DOI=10.1145358669.358692 http:doi.acm.org/10.1145/358669.358692) or    Kieper's paper (L. Kneip, D. Scaramuzza, R. Siegwart, “A Novel    Parameterization of the Perspective-Three-Point Problem for a Direct    Computation of Absolute Camera Position and Orientation”, Proc. of    The IEEE Conference on Computer Vision and Pattern Recognition    (CVPR), Colorado Springs, USA. June 2011) in combination with the    assumed focal length; physical pixel sizes; undistorted image    coordinates of the energy source at each point in the sequence; and    the exact positions of the robot at each point in the sequence to    determine the position of the camera relative to each set of points.-   8) Determine how tightly clustered the determined 6 degree of    freedom camera positions are. The camera positions should    theoretically be identical as the camera was rigidly mounted in    Step 2. This work uses the sum of standard deviations of each of the    6 degree of freedom positions as the tightness metric. This work    further enhanced this sensitivity by observing the sequence of    points from two rigid positions and using the sum of standard    deviations of the relative position of the cameras, which should of    course be constant.-   9) Numerically refine the assumed lens focal length until the    determined camera points are most tightly packed as determined by    the metric of Step 8. This work performs a simple coarse to fine    brute force search due to the non-continuous nature of the metric    and since it is mono-dimensional.

Next, the extrinsic positions of the cameras are determined by anextrinsic camera position determination module 26 of the processor 16 asfollows:

-   1) Characterise the camera's lens distortion. At least the radial    and tangential distortion parameters that are deemed significant are    required.-   2) Determine the camera's focal length (or add it to the list of    unknowns as described below).-   3) Rigidly mount the cameras on the desired mechanical platform for    the particular application, and rigidly mount the mechanical    platform such that the cameras have an unobstructed view of the    robot moving the energy source.-   4) Move the robot in a sequence of discrete points. Capture images    at each discrete point and find the center of the energy source in    the camera's image as described earlier. At each point also capture    the exact position of the energy source as returned by the robot.-   5) Determine an initial estimated position of the camera relative to    the robot. This work uses either prior knowledge of the physical set    up or a genetic algorithm to do this.-   6) Create a bundle of image based vectors. This is done by using the    distortion characterisation parameters together with the physical    pixel sizes and the focal length. This focal length can either be    known apriori or it can be added as the seventh unknown to be    determined.-   7) Create a bundle of geometry based vectors. These are created by    using the assumed 6 degree of freedom position of the camera    (Step 5) and the known position of the energy source at each point    in the sequence.-   8) Choose a metric to measure the similarity of the two bundles of    vectors. This work uses the sum of angles between corresponding    vectors as the metric.-   9) Refine the estimated 6 degree of freedom position of the camera    relative to the robot (and the focal length of the camera if not    determined apriori) to maximise the similarity of the two bundles of    vectors. This work uses either the Fletcher Reeves Conjugate    Gradient (Fletcher, R. and Reeves, C., “Function minimization by    conjugate gradients,” Computer Journal 7, 140-054(1964)) or Leapfrog    multi-dimensional non-linear numerical optimisation algorithms    (Snyman, J., “An improved version of the original leap-frog dynamic    method for unconstrained minimization: Lfop1(b),” Applied    Mathematics and Modelling 7, 216-218 (1983)) to do this.-   10) If multiple cameras are being calibrated, and if desired, choose    one camera or other known point as the reference system for the set    of cameras, and express the determined camera positions relative to    this point.

The mathematics underlying the above process steps are described belowin more detail.

The mathematical notation used below is as follows: A 3D vector,V_(abc), is a vector from point a directed towards point b expressed interms of its projections on orthogonal coordinate system c's axes.V_(abc) is used when the magnitude of the vector is unknown orunimportant.

T_(abc) represents the translation or displacement of point b relativeto point a. R_(ab) is a 3-by-3 Euler rotation matrix expressing therotation of an orthogonal axis system a relative to (and in terms of itsprojections on) an orthogonal axis system b. Individual elements of 3dimensional vectors are referred to as x, y or z whereas 2 dimensional(2D) vector's elements are referred to as horizontal (h) and vertical(v) to avoid confusion.

The relevant modules of the processor 16 carry out the followingfunctional steps on the captured image data:

-   1.1) Perform a connected components labeling of all pixels brighter    than a chosen threshold.-   1.2) Discard all components that do not meet size criteria as    determined by the energy source type and camera resolution.-   1.3) Discard all components that do not meet shape symmetry    criteria, i.e. for each connected component as follows:-   a) Fit the best fit straight line through each of the pixels of a    component:

$\begin{matrix}{{Equation}\mspace{14mu} 1} & \; \\{{\overset{\rightarrow}{x} = {\left( {A^{T}A} \right)^{- 1}A^{T}\overset{\rightarrow}{B}}}{{where}\text{:}}{A = \begin{bmatrix}x_{0} & 1 \\x_{1} & 1 \\\vdots & \vdots \\x_{N - 1} & 1\end{bmatrix}}{\overset{\rightarrow}{B} = \begin{bmatrix}y_{0} \\y_{1} \\\vdots \\y_{N - 1}\end{bmatrix}}\begin{matrix}{\left( {x_{i},y_{i}} \right) = {{ith}\mspace{20mu} {pixel}\mspace{14mu} {of}\mspace{14mu} {the}\mspace{14mu} {component}}} \\{{N = {{the}\mspace{14mu} {number}\mspace{14mu} {of}\mspace{14mu} {pixels}\mspace{14mu} {in}\mspace{14mu} {the}\mspace{14mu} {component}}},{and}} \\{\overset{\rightarrow}{x} = \left\lbrack {m,c} \right\rbrack^{T}} \\{= {{{the}\mspace{14mu} {coffecients}\mspace{14mu} {for}\mspace{14mu} {the}{\mspace{11mu} \;}{line}\mspace{14mu} y} = {{mx} + {c.}}}}\end{matrix}} & (1)\end{matrix}$

$\begin{matrix}{{{L_{A} = {L_{A,\max} - L_{A,\min}}}{{where}\text{:}}{{L_{A,\max} = \left. \max \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot \; {\overset{\rightarrow}{V}}_{L}^{n}} \right.},{{\forall_{i}L_{A,\min}} = \left. \min \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot {\overset{\rightarrow}{V}}_{L}^{n}} \right.},{{\forall_{i}{\overset{\rightarrow}{V}}_{L}^{n}} = \frac{{\overset{\rightarrow}{V}}_{L}}{{\overset{\rightarrow}{V}}_{L}}}}{{\overset{\rightarrow}{V}}_{L} = \left\lbrack {1,m} \right\rbrack^{T}}{i \in \left( {0,{N - 1}} \right)}}{{N = {{the}\mspace{14mu} {number}\mspace{14mu} {of}\mspace{14mu} {pixels}}},{and}}{m,{c = {{as}\mspace{14mu} {defined}\mspace{14mu} {in}{\mspace{11mu} \;}{Eq}\mspace{14mu} 1.}}}{L_{A} = {L_{A,\max} - L_{A,\min}}}{{where}\text{:}}{{L_{A,\max} = \left. \max \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot {\overset{\rightarrow}{V}}_{L}^{n}} \right.},{{\forall_{i}L_{A,\min}} = \left. \min \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot {\overset{\rightarrow}{V}}_{L}^{n}} \right.},{{\forall_{i}{\overset{\rightarrow}{V}}_{L}^{n}} = \frac{{\overset{\rightarrow}{V}}_{L}}{{\overset{\rightarrow}{V}}_{L}}}}{{\overset{\rightarrow}{V}}_{L} = \left\lbrack {1,m} \right\rbrack^{T}}{i \in \left( {0,{N - 1}} \right)}{{N = {{the}\mspace{14mu} {number}{\mspace{11mu} \;}{of}\mspace{14mu} {pixels}}},{and}}{m,{c = {{as}\mspace{14mu} {defined}\mspace{14mu} {in}{\mspace{11mu} \;}{Eq}\mspace{14mu} 1.}}}} & {{Equation}\mspace{14mu} 2}\end{matrix}$

-   c) Determine the width of the component perpendicular to the best    fit line.

$\begin{matrix}{{L_{P} = {L_{P,\max} - L_{P,\min}}}{{where}\text{:}}{{L_{P,\max} = \left. \max \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot \; {\overset{\rightarrow}{V}}_{P}^{n}} \right.},{{\forall_{i}L_{P,\min}} = \left. \min \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot {\overset{\rightarrow}{V}}_{P}^{n}} \right.},{{\forall_{i}{\overset{\rightarrow}{V}}_{P}^{n}} = \frac{{\overset{\rightarrow}{V}}_{P}}{{\overset{\rightarrow}{V}}_{P}}}}{{\overset{\rightarrow}{V}}_{P} = \begin{bmatrix}1 \\\frac{- 1}{m}\end{bmatrix}}{i \in \left( {0,{N - 1}} \right)}{{N = {{the}\mspace{14mu} {number}{\mspace{11mu} \;}{of}\mspace{14mu} {pixels}}},{and}}{m,{c = {{as}\mspace{14mu} {defined}\mspace{14mu} {in}{\mspace{11mu} \;}{Eq}\mspace{14mu} 1.}}}{L_{P} = {L_{P,\max} - L_{P,\min}}}{{where}\text{:}}{{L_{P,\max} = \left. \max \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot \; {\overset{\rightarrow}{V}}_{P}^{n}} \right.},{{\forall_{i}L_{P,\min}} = \left. \min \middle| {\left( {{\overset{\rightarrow}{P}}_{i} - \left\lbrack {0,c} \right\rbrack^{T}} \right) \cdot {\overset{\rightarrow}{V}}_{P}^{n}} \right.},{{\forall_{i}{\overset{\rightarrow}{V}}_{P}^{n}} = \frac{{\overset{\rightarrow}{V}}_{P}}{{\overset{\rightarrow}{V}}_{P}}}}{{\overset{\rightarrow}{V}}_{P} = \begin{bmatrix}1 \\\frac{- 1}{m}\end{bmatrix}}{i \in \left( {0,{N - 1}} \right)}{{N = {{the}\mspace{14mu} {number}\mspace{14mu} {of}\mspace{14mu} {pixels}}},{and}}{m,{c = {{as}\mspace{14mu} {defined}\mspace{14mu} {in}{\mspace{11mu} \;}{Eq}\mspace{14mu} 1.}}}} & {{Equation}\mspace{14mu} 3}\end{matrix}$

Next the processor compares the ratio of length (L_(A)) to width (L_(P))and discards if it is not within specified criteria.

-   1.4) The center of each component is determined using an appropriate    technique such as the two listed below for illustrative purposes:-   a) Center of gravity

$\begin{matrix}{{{{{{{{{{X_{COG} =_{\;}\frac{{\sum_{i = 0}^{N - 1}{{I\left( {x_{i},y_{i}} \right)} \times x_{i}}}\;}{\sum_{i = 0}^{N - 1}{I\left( {x_{i},y_{i}} \right)}}}Y_{COG}} =_{\;}\frac{{\sum_{i = 0}^{N - 1}{{I\left( {x_{i},y_{i}} \right)} \times y_{i}}}\;}{\sum_{i = 0}^{N - 1}{I\left( {x_{i},y_{i}} \right)}}}I\left( {x,y_{i}} \right)} = {{intensity}\mspace{14mu} {of}\mspace{14mu} {pixel}\mspace{14mu} {located}\mspace{14mu} {at}\mspace{20mu} \left( {x,y} \right)}},{\left( {x_{i},y_{i}} \right) = {{coordinates}\mspace{14mu} {of}\mspace{14mu} {the}\mspace{14mu} {component}}}}’}s\mspace{14mu} {ith}\mspace{14mu} {pixel}},{i \in \left( {0,{N - 1}} \right)},{and}}{N = {{number}\mspace{14mu} {of}\mspace{14mu} {pixels}\mspace{14mu} {in}\mspace{14mu} {the}\mspace{14mu} {{component}.}}}} & {{Equation}\mspace{14mu} 4}\end{matrix}$

-   b) Fitting an eclipse by (e.g.) minimising the following metric:

$\begin{matrix}{{{metric} = {{c_{0}\pi \; {ab}} + {c_{1}\left( {{C\; S} - {E\; S}} \right)} + {c_{2}\left( {{W\; S} - {2E\; S}} \right)}}}{where}{c_{n} = {{the}\mspace{14mu} {nth}\mspace{14mu} {weighting}\mspace{14mu} {term}}},{{C\; S} = {{sum}\mspace{14mu} {of}\mspace{14mu} {intensities}\mspace{14mu} {from}\mspace{14mu} {centroid}\mspace{14mu} {calculation}}},{{W\; S} = {\sum\limits_{h,{v \in W}}\; {I\left( {h,v} \right)}}},{{I\left( {h,v} \right)} = {{image}\mspace{14mu} {intensity}\mspace{14mu} {at}\mspace{14mu} 2D\mspace{14mu} {coordinate}\mspace{14mu} \left( {h,v} \right)}},{{E\; S} = {\sum\limits_{h,{v \in W}}\; \left\{ {{{\begin{matrix}{I\left( {h,v} \right)} & {{{{if}\mspace{14mu} C\; R}{E\; R}},} \\{\alpha \; {I\left( {h,v} \right)}} & {{{{if}\mspace{14mu} E\; R} < {C\; R}{{E\; R} + 1}},} \\0 & {{{{if}\mspace{14mu} C\; R} > {{E\; R} + 1}},}\end{matrix}C\; R} = {{{{{\langle{h,v}\rangle} - {\langle{E_{h},E_{v}}\rangle}}}E\; R} = {{\sqrt{\frac{a^{2}b^{2}}{\left( {b\; {\cos (\theta)}} \right)^{2} + \left( {a\; {\sin (\theta)}} \right)^{2}}}\alpha} = {{\left( {1 - \left( {{C\; R} - {E\; R}} \right)} \right)W} = {h \in \left( {{E_{h} - \left( {a + 3} \right)},{E_{h} + \left( {a + 3} \right)}} \right)}}}}},{{v \in {\left( {{E_{v} - \left( {a + 3} \right)},{E_{v} + \left( {a + 3} \right)}} \right)\left( {E_{h},E_{v}} \right)}} = {{centre}\mspace{14mu} {of}\mspace{14mu} {ellipse}}},{a = {{major}\mspace{14mu} {axis}\mspace{14mu} {of}\mspace{14mu} {ellipse}}},{b = {{minor}\mspace{14mu} {axis}\mspace{14mu} {of}\mspace{14mu} {ellipse}}},{\theta = {{angle}\mspace{14mu} {of}{\mspace{11mu} \;}{major}\mspace{14mu} {axis}\mspace{14mu} {from}\mspace{14mu} {horizontal}}}} \right.}}} & {{Equation}\mspace{14mu} 5}\end{matrix}$

Lens Distortion Characterisation

For lens distortion characterisation the fact that straight lines in thereal world must project onto straight lines in the image space afterdistortion has been corrected is used. To do this the robot (and itsattached energy source) is moved in a series of straight lines, stoppingat several points along each line for an image to be captured. Thisresults in N lines being captured each of which has M_(i), iε(0, N−1)points. These points are referred to as P_(i,j) ^(d) indicating theoriginal raw (i.e. distorted) image position of the jth point of the ithline.

Thereafter an arbitrary number of parameters for the Brown Lensdistortion model (Brown DC (1966). “Decentering distortion of lenses.”.Photogrammetric Engineering. 7: 444-462) can be numerically determined.

Any multi-dimensional numerical optimisation routine can be used,although some perform worse due to their mathematical mechanics and thehighly correlated nature of the parameters and residual noise inherentin the measurement of the input data.

In a prototype of the present invention, an augmented version on Brown'smodel is used whereby a radial gain scale factor is applied to theradial distortion parameters to facilitate variation that could beinduced by either non-orthogonality of the lens to the optical axis orother manufacturing defects. This does not affect the generality of thiswork as f(θ)=1 corresponds to the standard case published in literature.Without loss of generality we will assume, for illustrative purposes,that f(θ) is of the form: f(θ)=γ₁+γ₂ sin(θ−γ₃).

$\begin{matrix}{x_{u} = {x_{d} + {{f(\theta)}\left( {x_{d} - x_{c}} \right)\left( {{K_{1}r^{2}} + {K_{2}r^{4}} + \ldots} \right)} + {\left( {{P_{1}\left( {r^{2} + {2\left( {x_{d} - x_{c}} \right)^{2}}} \right)} + {2{P_{2}\left( {x_{d} - x_{c}} \right)}\left( {y_{d} - y_{c}} \right)}} \right) \times \left( {1 + {P_{3}r^{2}} + \ldots} \right)}}} & {{Equation}\mspace{14mu} 6} \\\left. {y_{u} = {y_{d} + {{f(\theta)}\left( {y_{d} - y_{c}} \right)\left( {{K_{1}r^{2}} + {K_{2}r^{4}} + \ldots} \right)} + {\left( {{2{P_{1}\left( {x_{d} - x_{c}} \right)}\left( {y_{d} - y_{c}} \right)} + {P_{2}\left( {r^{2} + {2\left( {y_{d} - y_{c}} \right)^{2}}} \right)}} \right) \times \left( {1 + {P_{3}r^{2}} + \ldots} \right)}}} \right) & \; \\{\mspace{85mu} {{{{where}\text{:}}\mspace{85mu} {{\left( {x_{u},y_{u}} \right) = {{undistorted}\mspace{14mu} {image}{\mspace{11mu} \;}{point}}},\mspace{20mu} {\left( {x_{d},y_{d}} \right) = {{distorted}\mspace{14mu} {image}\mspace{14mu} {point}}},\mspace{20mu} {\left( {x_{c},y_{c}} \right) = {{centre}\mspace{14mu} {of}\mspace{14mu} {distortion}}},\mspace{20mu} {K_{n} = {N^{th}\mspace{14mu} {radial}\mspace{14mu} {distortion}\mspace{14mu} {coefficient}}},\mspace{20mu} {P_{n} = {N^{th}\mspace{14mu} {tangential}\mspace{14mu} {distortion}{\mspace{11mu} \;}{coefficient}}},\mspace{20mu} {\theta = {\tan^{- 1}\left( \frac{y_{d} - y_{c}}{x_{d} - x_{c}} \right)}}}\mspace{20mu} {{f(\theta)} = {{the}\mspace{14mu} {radial}\mspace{14mu} {gain}}}},\mspace{20mu} {r = \sqrt{\left( {x_{d} - x_{c}} \right)^{2} + \left( {y_{d} - y_{c}} \right)^{2}}},\mspace{20mu} {{{and}\mspace{11mu} \ldots} = {{an}\mspace{14mu} {infinite}{\mspace{11mu} \;}{{series}.}}}}} & \;\end{matrix}$

A metric is used to measure how straight a set of lines is, this metricdetermines the best fit straight line through each captured straightline's points (see Eq. 1) and then determines the RMS distance of thepoints from their straight lines. The procedure used to determine thebest fit straight line is given in Eq. 1. This metric is:

$\begin{matrix}{{metric} = \sqrt{\frac{1}{{\sum_{n = 0}^{n < N_{L}}M_{n}}\;}{\sum\limits_{n = 0}^{n < N_{L}}\; {\sum\limits_{m = 0}^{m < M_{n}}\; \left( {\left( {{\overset{\rightarrow}{P}}_{n,m}^{u} - \left\lbrack {0,c_{n}} \right\rbrack^{T}} \right) \cdot {\overset{\rightarrow}{d}}_{n}} \right)^{2}}}}} & {{Equation}\mspace{14mu} 7} \\{\mspace{79mu} {where}} & \; \\{{N_{L} = {{the}{\mspace{11mu} \;}{number}\mspace{14mu} {of}\mspace{14mu} {straight}\mspace{14mu} {lines}\mspace{14mu} {in}\mspace{14mu} {the}\mspace{14mu} {data}\mspace{14mu} {captured}}},} & \; \\{\mspace{79mu} {{M_{n} = {{the}\mspace{14mu} {number}\mspace{14mu} {of}\mspace{14mu} {points}\mspace{14mu} {along}{\mspace{11mu} \;}{the}\mspace{14mu} {nth}\mspace{14mu} {line}}},}} & \; \\\begin{matrix}{{{\overset{\rightarrow}{d}}_{n} = {{unit}\mspace{14mu} {direction}\mspace{14mu} {vector}\mspace{14mu} {orthogonal}\mspace{14mu} {to}\mspace{14mu} {the}\mspace{14mu} {RMS}{\mspace{11mu} \;}{line}\mspace{14mu} n}},} \\{= \frac{\left\lbrack {1,m} \right\rbrack^{T}}{\left\lbrack {1,m} \right\rbrack }}\end{matrix} & \; \\{m_{n},{c_{n} = {{coefficient}\mspace{14mu} {of}\mspace{14mu} {best}\mspace{14mu} {fit}{\mspace{11mu} \;}{line}\mspace{14mu} n\mspace{14mu} {as}\mspace{14mu} {per}\mspace{14mu} {{Eq}.\mspace{11mu} 1}}},{and}} & \; \\{\mspace{85mu} {{\overset{\_}{P}}_{n,m}^{u} = {{the}\mspace{14mu} {undistorted}\mspace{14mu} {point}\mspace{14mu} {in}\mspace{14mu} {{question}.}}}} & \;\end{matrix}$

The following steps are then performed in order to determine theresidual distortion resulting from a given set of parameters:

-   2.1) Scale the received parameters from gradient desensitized space:    -   x_(c)=k_(xc)x_(c) ^(n)    -   y_(c)=k_(yc)y_(c) ^(n)    -   K_(i)=K_(Ki)K_(i) ^(n),∀iε(1, N_(K))    -   P_(i)=K_(Pi)P_(i) ^(n),∀iε(1, N_(P))    -   γ_(i)=K_(γi)γ_(i) ^(n),∀iε(1, N_(γ))    -   where:        -   N_(K)=the number of radial parameters,        -   N_(P)=the number of tangential parameters,        -   N_(γ)=the number of asymmetry parameters,    -   (k_(xc), k_(yc))=distortion center scaling parameters,        -   k_(Ki)=scale factor for ith radial parameter,        -   k_(Pi)=scale factor for ith tangential parameter,        -   k_(γi)=scale factor for ith asymmetry parameter,    -   (x_(c) ^(n), y_(c) ^(n))=received normalised distortion center,        -   K_(i) ^(n)=received normalised radial parameter,        -   P_(i) ^(n)=received normalised tangential parameter, and        -   γ_(i) ^(n)=received normalised asymmetry parameter.

Equation 8

2.2) Use the scaled parameters and to undistort each point of every linecaptured for undistortion, i.e.

{right arrow over (P)} _(i,j) ^(U) =F _(undit)({right arrow over (P)}_(i,j) ^(D) ,x _(c) ,y _(c) ,K ₁ . . . K _(N) _(k) ,P ₁ . . . P _(N)_(p) ;γ₁ . . . γ_(N) _(γ) ),∀iε(0,N _(L)−1),∀jε(0,M _(n)−1)

-   -   where:    -   F_(undist)=the augmented Brown model given in Eq.6.

Equation 9

-   2.3) Use Eq. 1 to determine the best fit line through the    undistorted points of each line in the input dataset.-   2.4) Use Eq. 7 to determine the RMS perpendicular distance of the    points from their lines.

The procedure for numerical optimisation of the distortioncharacterisation parameters is given below.

A numerical optimisation of distortion characterisation parameters isthen calculated by the processor 16 as follows:

-   3.1) Decide which parameters are going to be optimised. That is:    choose the number of radial and tangential distortion parameters;    whether the image center is used or if optimal distortion center    will be found; and select a radial gain function.-   3.2) Select a starting value for each parameter. Three common ways    of doing this are:    -   a) Set all the parameters to 0    -   b) Use prior knowledge to select approximate starting values    -   c) Specify a range for each parameter and perform a coarse        global optimisation, such as brute force or a genetic algorithm.-   3.3) Scale each of the input parameters, so that the gradient is    equally sensitive to a constant size perturbation in each dimension.    This allows for a more accurate gradient estimation by the local    optimisation procedure resulting in better characterisations. With    reference to Eq. 8, Eq 10 shows the scaling procedure    -   x_(c) ^(n)=x_(c) ^(s)/k_(xc)    -   y_(c) ^(n)=y_(c) ^(s)/k_(yc)    -   K_(i) ^(n)=K_(i) ^(s)/k_(Ki),∀iε(1, N_(K))    -   P_(i) ^(n)=P_(i) ^(s)/k_(Pi),∀iε(1, N_(P))    -   γ_(i) ^(n)=γ_(i) ^(s)/k_(γi),∀iε(1, N_(γ))    -   where:    -   (x_(c) ^(s), y_(c) ^(s))=starting distortion center values,        -   K_(i) ^(s)=starting radial correction parameter values,        -   P_(i) ^(s)=starting tangential correction parameter values,            and        -   γ_(i) ^(s)=starting asymmetry correction parameter values.

Equation 10.

-   3.4) Use a local optimisation routine to numerically refine the    normalised starting parameters given in Eq. 10, the value to be    minimised is specified in Algorithm 2. Specifically Leapfrog and    Fletcher-Reeves are used in this work to perform the minimisation.-   3.5) Denormalise the returned distortion characterisation values    using Eq. 8.

Focal Length Determination

The processor 16 determines the focal parameters, without adding theextrinsic parameters to the bundle of parameters to be numericallyrefined.

Determining the focal length simultaneously with the extrinsicparameters will be described below. Performing the focal lengthdetermination independently decreases the dimensionality of theextrinsic parameter determination and removes the ambiguity thatprevents the focal length being determinable when orthogonally viewing aplanar pattern (or sequence of robot movements in a plane).

A. The Tetrahedron Perspective Problem

This calibration makes use of the three point perspective problem whichis formally stated in RANSAC paper and KIEPER'S paper (both referencedabove) and restated here in words for explanatory purposed. When acamera views three points that have known distances between them, theorientation and the translation of the camera relative to the threepoints can be solved analytically.

This is done by calculating unit direction vectors from vectors from thecamera to each of the points (see Eq. 20) and then calculating the anglebetween these direction vectors (via the vector dot product).

The cosine rule given in Eq. 11 is the generalisation of Pythagorus'theorem to non-right angled triangles.

a ² =b ² +c ²−2bc cos(θ_(a))

-   -   where:    -   a, b, e=the lengths of the sides of a triangle, and        -   θ_(a)=the angle opposite side a.

Equation 11

Using the cosine rule, and recalling that the dot product of two vectorsis equal to the cosine of the angle between them, one can set up aseries of simultaneous equations expressing the three unknown lengths ofthe vectors from the camera to each viewing point, in terms of the unitvectors from the camera to the points and the known distances betweenthe points of the triangle as follows:

L _(1,2) +L _(c,1) ² +L _(c,2) ²−2L _(c,1) L _(c,2)({right arrow over(U)} _(c1c) •{right arrow over (U)} _(c2c))

L _(2,3) +L _(c,2) ² +L _(c,3) ²−2L _(c,2) L _(c,3)({right arrow over(U)} _(c2c) •{right arrow over (U)} _(c3c))

L _(1,3) +L _(c,1) ² +L _(c,3) ²−2L _(c,1) L _(c,3)({right arrow over(U)} _(c1c) •{right arrow over (U)} _(c3c))

where:

-   -   L_(i,j)=the distance between points i and j of the triangle        ∀i,jε[1,3],    -   L_(c,i)=the distance between the camera and point i of the        triangle ∀i,ε[1,3],    -   {right arrow over (U)}_(icc)=the unit vector pointing from the        camera to point i of the triangle; as determined by Eq.20.

Equation 12

Eq. 12 has four sets of solutions (the determination of which is givenin RANSAC paper and KIEPERS paper (both referenced above), although notall solutions are purely real on the complex plane. In order to selectwhich solution is the correct one, a fourth point which is out of theplane of the first three points and whose translation relative to theother point is known, is required. The four points then constitute atetrahedron, with the fourth point at the apex. For each real solutionthe position of the camera relative to the triangle of points (i.e. thebase of the tetrahedron) is calculated, and then the position of thefourth point relative to the camera is calculated. The vector to thiscalculated position of the fourth point is compared to the vectorcalculated from the image coordinates of the fourth point, and thesolution that has the smallest angle between these two vectors isselected as the correct solution.

This entire procedure is summarised in Eq. 13 below.

({right arrow over (T)} _(tct) ,R _(ct))=f _(t)({right arrow over (U)}_(c1c) ,{right arrow over (U)} _(c2c) ,{right arrow over (U)} _(c3c),{right arrow over (U)} _(c4c) ,{right arrow over (T)} _(t1t) ,{rightarrow over (T)} _(t2t) ,{right arrow over (T)} _(t3t) ,{right arrow over(T)} _(t4t))

where:

-   -   f_(t)=the procedures described earlier in this section,    -   {right arrow over (T)}_(tct)=the translation of the camera        relative to the tetrahedron in the tetrahedron's axis system,    -   R_(ct)=the Euler rotation matrix of the cameras axis system        relative to the tetrahedron's,    -   {right arrow over (U)}_(cic)=the unit vector pointing from the        camera to point i of the tetrahedron, as per Eq.20, and    -   {right arrow over (T)}_(tit)=the translation of the ith point of        the tetrahedron in its axis system.

Equation 13

Focal length determination makes use of the tetrahedron problem detailedabove and summarised in Eq. 13. Note that Eq. 13 makes use of Eq. 20below which is dependent on the distortion parameters of the lens whichare assumed to have already been calibrated via the methods described inalgorithm 3 and the focal length which is the subject of thischaracterisation.

The robot is used to place the energy source in the field of view of thecamera in a number of positions such that a number of tetrahedrons arecreated. In the exemplary embodiment 20 tetrahedrons were created infour groups of five. Each group had the tetrahedron centered located ata different location, with the groups' central positions forming a ‘+’.At each location the tetrahedrons were angularly offset such that thecamera's optical axis was not normal to the base of the tetrahedron. Thetetrahedrons in a group were respectively angled to lookup-to-the-right, down-to-the-right, down-to-the-left, and up-to-the-leftas seen from the camera's point of view.

To determine the focal length the camera was placed in two positions toview the tetrahedrons. The camera was rigidly placed in the firstposition and then viewed all the tetrahedrons, after which it was placedin the second position and it again viewed the robot moving through theexact same tetrahedrons. The camera was stationary at each positionwhile viewing the tetrahedrons. This means that the relativedisplacement of the camera was constant. Since the robot was used tomove the energy source to each subsequent position for each tetrahedron,the translations of the tetrahedron points are known. If the distortionparameters are already known then the focal length remains the onlyparameter required by Eq. 13.

For an assumed focal length the position and orientation of the camerarelative to the robot reference (which is the axis system in which theenergy source's translations (i.e. the T_(icc)'s of Eq. 13) are known)can be calculated.

At the correct focal length the locus of calculated camera positionswill be the smallest. This in itself can be used as a metric todetermine the ideal focal length, however this sensitivity to focallength is increased by comparing the relative position and orientationof the camera at the second position compared to the camera at the firstposition for each tetrahedron. This is possible due to the highrepeatability of the robots used (in the exemplary embodiment and ABBIRB120). The variation in the relative positions is the metric that isminimised. The calculation of this metric is given in Algorithm 4 below.

The variation of the camera relative to 6 degree of freedom positionsfor a tetrahedron set and focal length is calculated next by theprocessor as follows:

-   4.1) Calculate the unit vectors from the processed images of the    energy source, the distortion parameters, and the specified focal    length as described in Eq. 20:

{right arrow over (U)} _(c) _(a,j) _(ic) _(a,j) =f _(UV)({right arrowover (I)} _(a,i,j) ^(d),Distortion_Parameters,Focal_(—) Len)

{right arrow over (U)} _(c) _(b,j) _(ic) _(b,j) =f _(UV)({right arrowover (I)} _(b,i,j) ^(d),Distortion_Parameters,Focal_(—) Len)

-   -   where:        -   iε[1, 4] and denotes the number of the vertex in the            tetrahedron,        -   jε[1, N_(tet)] and denotes the tetrahedron number.        -   N_(tet)=the number of tetrahedrons captured        -   f_(UV)=creates a unit vector from an image coordinate and            intrinisic camera parameters (i.e. Eq. 20),        -   {right arrow over (I)}_(x,i,j) ^(d)=the image coordinates of            the captured center of the energy source with the robot arm            at vertex i of tetrahedron j as viewed from camera portion            x, and        -   {right arrow over (U)}_(c) _(x,j) _(ic) _(x,j) =the unit            vector from the camera at position x towards the its vertex            of tetrahedron j.

Equation 14

-   4.2) Calculate the position of the camera at each position for all    the tetrahedrons (note that the robot reference is used as the    tetrahedron axis ∴{right arrow over (T)}_(t,xr)≡{right arrow over    (T)}_(rxr)):

({right arrow over (T)} _(r) c _(a,i) r,R _(c) _(a,i) _(r))=f_(t)({right arrow over (U)}c _(b,i) jc _(b,i) ∀jε[1,4],{right arrow over(T)}t _(i) jr∀jε[1,4]

({right arrow over (T)} _(r) c _(b,i) r,R _(c) _(b,i) _(r))=f_(t)({right arrow over (U)}c _(b,i) jc _(b,i) ∀jε[1,4],{right arrow over(T)}t _(i) jr∀jε[1,4]

-   -   where:        -   {right arrow over (T)}_(t) _(j) _(ir)==translation of vertex            i of tetrahedron j expressed relative to the robot            reference,        -   {right arrow over (T)}_(rc) _(x,i) _(r)=translation of the            camera at position x expressed relative to the robot            reference as calculated using tetrahedron and        -   R_(c) _(a,i) _(r)=orientation of the camera at position x            expressed relative to the robot reference as calculated            using tetrahedron i.

Equation 15

-   4.3) For each tetrahedron, calculate the position of camera at    position b relative to camera at position a

R _(c) _(b,i) _(c) _(a,i) =R _(c) _(a,i) _(r) ^(T) R _(c) _(b,i) _(r)

{right arrow over (T)} _(c) _(b,i) _(c) _(a,i) _(r) ={right arrow over(T)} _(rc) _(b,i) _(r) −{right arrow over (T)} _(rc) _(a,i) _(r)

-   -   where    -   R_(c) _(b,i) _(c) _(a,i) =the orientation of the camera at        position b relative to the orientation at position a, calculated        using tetrahedron i, and    -   {right arrow over (T)}_(c) _(b,i) _(c) _(a,i) _(r)=the        translation of the camera at position b relative to the        translation at position a, calculated using tetrahedron i.

Equation 16

-   4.4) Iterate over the results and extract the yaw pitch and roll    angles from the Euler rotation matrices of the relative orientations    of the camera in the two positions. (i.e. from Rc_(bj)c_(aj)).-   4.5) Calculate the standard deviation of the X ordinate of the    relative translation of the cameras as calculated over all the    tetrahedron. Repeat for the Y ordinate, the Z ordinate and the yaw,    pitch and roll positions just calculated.-   4.6) Calculate a weighted sum of the standard deviations to use as    the metric:

metric=K ₀ X _(stddev) +K ₁ Y _(stddev) +K ₂ Z _(stddev) +K ₃ Yaw_(stddev) +K ₄Pitch_(stddev) +K ₅ Roll _(stddev)

-   -   where:        -   K_(n)=the nth weighting coefficient.

Equation 17

In the exemplary embodiment weighting values for Eq. 17 of K₀=K₁=K₂=1:0and K₃=K₄=K₅=10:0 were used.

In order to find the ideal focal length a range centered around thelens's claimeddesigned focal length needs to be searched for the minimumvalue of Eq. 17. This can take the form of any line search techniquesuch as Powell's method and Newton's method for finding zero crossingsof the derivative. As the metric was seen to be non-continuous and isone-dimensional, the exemplary embodiment consists of a coarse-to-finebrute force search.

Extrinsic Parameter Determination

The 6 degree of freedom position of the camera relative to the robot,the focal length (if not determined as in Section VI) and the opticalintersection pixel position (also known as the principal point) arerequired. In order to do this either of the metrics expressed in Eq 18or Eq 19 may be numerically optimized using a robust algorithm such asFletcher-Reeves or Leapfrog (both referenced above). The first metric isslower but more accurate due to the increased sensitivity toalmost-parallel vectors provided by the (computationally intensive)inverse cosine function.

$\begin{matrix}{{{Metric} = {\sum\limits_{i = 0}^{n - 1}\; \left( {\cos^{- 1}\left( {{\overset{\rightarrow}{U}}_{cic}^{1} \cdot {\overset{\rightarrow}{U}}_{cic}^{2}} \right)} \right)}}{{Metric} = {\sum\limits_{i = 0}^{n - 1}\; \left( {1.0 - {{\overset{\rightarrow}{U}}_{cic}^{1} \cdot {\overset{\rightarrow}{U}}_{cic}^{2}}} \right)}}} & {{Equations}\mspace{14mu} 18\mspace{14mu} {and}\mspace{14mu} 19}\end{matrix}$

The metrics are a comparison between two bundles of vectors. Eq 20 belowshows how one of the vector bundles can be generated from the images ofthe energy source mounted on the robot, once the images have beenprocessed as described in Section IV, and then undistorted using Eq 6and the final distortion characterisation parameters determined inSection V. The pixel dimensions are assumed known from the data sheet,thus leaving the focal length (potential) and optical axis intersectionpoint as the only unknowns. Good initial guesses for the numericaloptimization are the manufacturer's claimed focal length and thedistortion centre respectively.

$\begin{matrix}{{{{{{{{{{{{{{{\overset{\rightarrow}{I}}_{i}^{u}{f^{undistort}\left( {\overset{\rightarrow}{I}}_{i}^{d} \right)}}{{\overset{\rightarrow}{V}}_{cic} = \begin{bmatrix}{Focal\_ Len} \\{\left( {{PP}_{h} - I_{i_{h}}^{u}} \right){pix\_ w}} \\{\left( {{PP}_{v} - I_{i_{v}}^{u}} \right){pix\_ h}}\end{bmatrix}}{U_{cic}^{1} = {V_{cic}\text{/}{V_{cic}}}}{where}\text{:}}{{\overset{\rightarrow}{I}}_{i}^{d} = {{the}\mspace{14mu} 2D\mspace{14mu} {image}\mspace{14mu} {pixel}\mspace{14mu} {position}\mspace{14mu} {of}\mspace{14mu} {energysource}\mspace{14mu} {at}\mspace{14mu} {position}\mspace{14mu} i}}},{f^{undistort} = {{the}\mspace{14mu} {predetermined}\mspace{14mu} {lens}\mspace{20mu} {undistortion}\mspace{20mu} {characterization}\mspace{14mu} {function}}},{{P\; P} = {{the}\mspace{14mu} {optical}\mspace{14mu} {axis}\mspace{14mu} {intersection}\mspace{14mu} {pixel}\mspace{14mu} {position}}},{{pix\_ w} = {{the}\mspace{14mu} {width}\mspace{14mu} {of}\mspace{14mu} {the}\mspace{14mu} {pixels}\mspace{14mu} {on}\mspace{14mu} {the}\mspace{14mu} {camera}}}}’}s\mspace{14mu} {imager}},{{pix\_ h} = {{the}\mspace{14mu} {height}\mspace{14mu} {of}\mspace{14mu} {the}\mspace{14mu} {pixels}\mspace{14mu} {on}\mspace{14mu} {the}\mspace{14mu} {camera}}}}’}s\mspace{14mu} {imagers}},{{Focal\_ Len} = {{the}\mspace{14mu} {exact}\mspace{14mu} {focal}\mspace{14mu} {length}\mspace{14mu} {of}\mspace{14mu} {the}\mspace{14mu} {camera}}}}’}s\mspace{14mu} {lens}},{and}}{U_{icc}^{1} = {a{\mspace{11mu} \;}{unit}\mspace{14mu} {vector}\mspace{14mu} {pointing}\mspace{14mu} {from}\mspace{14mu} {the}\mspace{14mu} {camera}\mspace{14mu} {to}\mspace{14mu} {position}\mspace{14mu} {i.}}}} & {{Equation}\mspace{14mu} 20}\end{matrix}$

The second vector bundle is calculated via Eq 21. It is assumed that theposition of the energy source ({right arrow over (T)}_(rir)) at eachrobot position is known. Thereafter the unknown spatial offset of therobot reference ({right arrow over (T)}_(crc)) and the (also unknown)Euler rotation of the robot reference relative to the camera (R_(rc))are used to determine and then normalize a vector to each energy sourceposition. Also note that there is a singularity in both metrics if aplanar optical reference jig is used and is placed perpendicular to thecamera's optical axis.

{right arrow over (T)} _(cic) =R _(rc) {right arrow over (T)} _(rir)+{right arrow over (T)} _(crc)

{right arrow over (U)} _(cic) ² ={right arrow over (T)} _(cic) /∥{rightarrow over (T)} _(icc)∥

-   -   where:        -   {right arrow over (T)}_(crc)=the spatial position of the            robot reference relative to the camera,        -   {right arrow over (T)}_(rir)=the spatial offset of the            energy source relative to the robot reference,        -   R_(rc)=the Euler rotation matrix of the robot reference            relative to the camera, and        -   {right arrow over (U)}_(cic) ²=unit vector pointing from the            camera to energy source at position i.

Equation 21

The following algorithm explains how to find the extrinsic parametersgiven a corresponding set of positions of the energy source obtainedfrom the robot ({right arrow over (T)}_(rir)) and the pixel coordinatesof where the energy source is in the camera's image ({right arrow over(I)}_(i) ^(d)) as obtained by the methods described in algorithm 3. Itis assumed that the distortion characterisation has already beendetermined as described above.

A determination of the camera extrinsic parameters is now completed bythe processor as follows:

-   5.1) Choose the starting values for the nine parameters being    optimised. For the focal length the claimed/designed value is a good    starting point, and for the principal point the distortion center is    a good starting point. For the three translation and three    orientation parameters a crude physical measurement can serve as the    starting point. Alternatively a coarse global optimisation technique    such as a sparse brute force sampling or (as in the exemplary    embodiment) a genetic algorithm can be used to generate the starting    values.-   5.2) Choose the gradient desensitising scale factors for each of the    parameters. The values used in the exemplary embodiment are listed    below.    -   γ_(FLen) Focal length scale factor: 10⁻¹ if the focal length (in        min) is being determined, or removed from the optimisation        parameter bundle if determined via Algorithm 3.    -   γ_(PPh),γ_(PPv) The principal point horizontal and vertical        scale factors, set to 10² if the principal point (in pixels) is        being determined, or removed from the parameter bundle if the        distortion center is being used.    -   γ_(Yaw) scale factor, set to 10⁰ with the angle expressed in        radians.    -   γ_(Pitch) scale factor, set to 10⁰ with the angle expressed in        radians.    -   γ_(Roll) Roll scale factor, set to 10⁰ with the angle expressed        in radians.    -   γ_(X) X scale factor, set to 10⁻³ with the displacement        expressed in mm.    -   γ_(Y) Y scale factor, set to 10⁻³ with the displacement        expressed in mm.    -   γ_(Z) Z scale factor, set to 10⁻³ with the displacement        expressed in mm.        -   a) γ_(FLen) Focal length scale factor: 10⁻¹ if the focal            length (in mm) is being determined, or removed from the            optimisation parameter bundle if determined via Algorithm 3,        -   b) γ_(PPh),γ_(PPv) The principal point horizontal and            vertical scale factors, set to 10² if the principal point            (in pixels) is being determined, or removed from the            parameter bundle if the distortion center is being used.        -   c) γ_(Yaw) Yaw scale factor, set to 10⁰ with the angle            expressed in radians.        -   d) γ_(Pitch) Pitch scale factor, set to 10⁰ with the angle            expressed in radians.        -   e) γ_(Roll) Roll scale factor, set to 10⁰ with the angle            expressed in radians,        -   f) γ_(X) X scale factor, set to 10⁻³ with the displacement            expressed in mm.        -   g) γ_(Y) Y scale factor, set to 10⁻³ with the displacement            expressed in mm.        -   h) γ_(Z) Z scale factor, set to 10⁻³ with the displacement            expressed in mar.-   5.3) Divide each parameter by their corresponding scale factor to    produce the normalised parameters (denoted by the superscript n).-   5.4) Numerically refine the scaled parameters using the metric    described in Algorithm 6. Any non-linear multidimensional numerical    local optimisation may be used. The exemplary embodiment makes use    of Leapfrog or Fletcher-Reeves (both referenced above).-   5.5) Multiply the returned scaled parameters by their corresponding    scale factors to yield the position of the robot relative to the    camera expressed in the camera's axis (R_(rc)) and the translation    of the robot relative to the camera expressed in the camera's axis    ({right arrow over (T)}_(crc))-   5.6) Calculate the position of the camera relative to the robot:

R _(cr) =R _(rc) ^(T)

{right arrow over (T)} _(rcr) =−R _(cr) {right arrow over (T)} _(crc)

where:

-   -   R_(c)r=Euler rotation matrix of the camera relative to the        robot's reference axis, and    -   {right arrow over (T)}_(rcr)=the translation of the camera        relative to the robot reference axis, expressed in the robot's        reference axis.

R _(cr) =R _(cr) ^(T)

{right arrow over (T)} _(rcr) =−R _(cr) {right arrow over (T)} _(crc)

where:

-   -   R_(c)r=Euler rotation matrix of the camera relative to the        robot's reference axis, and    -   {right arrow over (T)}_(rcr)=the translation of the camera        relative to the robot reference axis, expressed in the robot's        reference axis.

Equation 22

An extrinsic parameter refinement metric is now calculated:

-   6.1) Multiply the received parameters by their corresponding scale    factors:

$\begin{matrix}{{Focal\_ Len} = \left\{ {{\begin{matrix}{Focal\_ Len} & {{if}\mspace{14mu} {not}\mspace{14mu} {part}\mspace{14mu} {of}\mspace{14mu} {optimised}\mspace{14mu} {parameters}} \\{\;_{\gamma_{FLen}}{Focal\_ Len}^{n}} & {{if}\mspace{20mu} {part}\mspace{14mu} {of}\mspace{14mu} {optimised}\mspace{14mu} {parameters}}\end{matrix}{PP}_{h}} = \left\{ {{\begin{matrix}{PP}_{h} & {{if}\mspace{14mu} {not}\mspace{14mu} {part}\mspace{14mu} {of}\mspace{14mu} {optimised}\mspace{14mu} {parameters}} \\{\;_{\gamma_{PPh}}{PP}_{h}^{n}} & {{if}\mspace{20mu} {part}\mspace{14mu} {of}\mspace{14mu} {optimised}\mspace{14mu} {parameters}}\end{matrix}{PP}_{v}} = \left\{ {{\begin{matrix}{PP}_{v} & {{if}\mspace{14mu} {not}\mspace{14mu} {part}\mspace{14mu} {of}\mspace{14mu} {optimised}\mspace{14mu} {parameters}} \\{\;_{\gamma_{PPv}}{PP}_{v}^{n}} & {{if}\mspace{20mu} {part}\mspace{14mu} {of}\mspace{14mu} {optimised}\mspace{14mu} {parameters}}\end{matrix}\mspace{20mu} {Yaw}} =_{\gamma_{Yaw}}{{{Yaw}_{n}\mspace{20mu} {Pitch}} =_{\gamma_{Pitch}}{{{Pitch}_{n}\mspace{20mu} {Roll}} = {{\gamma_{Roll}{Roll}_{n}\mspace{20mu} X} = {{\gamma_{X}X_{n}\mspace{20mu} Y} = {{\gamma_{X}X_{n}\mspace{20mu} Z} = {\gamma_{Z}Z_{n}}}}}}}} \right.} \right.} \right.} & {{Equation}\mspace{14mu} 23}\end{matrix}$

-   6.2) Calculate the Euler rotation matrix of the robot relative to    the camera (R_(rc)) from the Yaw, Pitch and Roll angles.-   6.3) Concatenate the X, Y, and Z values to form the translation of    the robot relative to the camera: ({right arrow over (T)}_(crc)).-   6.4) Calculate the image based bundle of unit vectors using Eq. 20,    the focal length, the principal point, pixel sizes, distortion    correction parameters with Eq. 6 and the set of pixel positions of    the energy source centers ({right arrow over (I)}_(i) ^(d)). (This    need be performed only once if the focal length and principal point    are both not part of the optimised parameter bundle).-   6.5) Calculate the bundle vectors that are based on the camera    position relative to the robot using Eq. 21, the set or positions of    the energy source positions in the robot's axis ({right arrow over    (T)}_(ric)), the current estimation of the robots orientation    relative to the camera (R_(rc)) and the current estimation of the    translation of the robot relative to the camera ({right arrow over    (T)}_(crc)).-   6.6) Measure the similarity of the two vector bundles using Eq. 18.

It will be appreciated that the method and system described above relateto a process to calibrate a camera and makes use of an exemplaryembodiment to do so. The exemplary embodiment is only one possibleinstantiation of the intellectual property claimed in this patent.Specifically:

-   1) Making use of a robotic arm to capture points along straight    lines to calibrate the distortion parameters of a camera by fitting    a lens distortion model to ensure that the straight lines in object    space project on to straight lines in image space. The distortion    model can include but is not limited to the Brown Lens Distortion    Model (referenced above)—with or without a radial gain function, and    neural networks.-   2) Determining the focal length by minimising the locus of (3 or 6    dimensional) positions of a camera calculated from viewing a three    or more planar points and one or more points outside of the plane.-   3) Determining the (3 or 6 dimensional) position of a camera by    observing a set of images of an energy source as moved by a robot,    and comparing a bundle of vectors based on analysis of the images to    a second bundle of vectors calculated from a hypothesized (3 or 6    dimensional) position of the camera.

Once the camera has been calibrated the camera can be used find theprecise location of an energy source.

1-22. (canceled)
 23. A system for calibrating a camera, the systemincluding: an energy source and a camera to be calibrated, with at leastone of the energy source and the camera being mounted on a mechanicalactuator so that it is movable relative to the other; a processorconnected to the energy source, the mechanical actuator and the camera,the processor programmed to: control the mechanical actuator to move atleast one of the energy source and the camera relative to the otherthrough a plurality of discrete points on a calibration target pattern;at each of the discrete points, control the camera to take a digitalimage; perform a lens distortion characterisation on each image;determine a focal length of the camera including any lens connected tothe camera; and determine an extrinsic camera position for each image.24. A system according to claim 23, wherein the processor performs thelens distortion characterisation by: choosing a distortion correctionmodel and determine an initial estimate of parameters for this model tocorrect an observed distortion; choosing a line straightness metric,which measures and quantifies co-linear points along a sampled line; andusing the line straightness metric and numerically refining the initialestimated parameters until the lines in the distortion corrected image.25. A system according to claim 23, wherein the processor determines afocal length by: selecting an initial focal length; using algorithms incombination with the initial focal length, physical pixel sizes,undistorted image coordinates of the energy source at each point in thesequence, and the exact positions of the mechanical actuator at eachpoint in the sequence to determine the position of the camera relativeto each discrete point; determine how tightly clustered the camerapositions are; and numerically refine the initial focal length until thedetermined discrete points are most tightly packed.
 26. A systemaccording to claim 23, wherein the processor determines an extrinsiccamera position by: creating a bundle of geometry based vectors;creating a bundle of image processing based vectors; choosing a metricto measure the similarity of the two bundles of vectors; and refine anestimated position of the camera relative to the energy source tomaximise the similarity of the bundles of vectors.
 27. A systemaccording to claim 23, wherein after the digital images have beencaptured, the processor further performs the following imagingprocessing steps: determine which regions of adjacent pixels in an imagehave an intensity above a chosen threshold value; generate a list ofsuch regions and the pixels which belong to each region together withthe pixels' coordinates and intensities; remove from this list anyregions which have either too few or too many constituent pixels asdetermined by characteristics of the camera, lens and energy source;remove from the list all regions that do not meet shape criteria; anddetermine a center of the largest remaining region.
 28. A systemaccording to claim 27, wherein the processor determines the center byfitting an ellipse to the region's pixel and using its center or bycalculating the center of gravity of the pixels in the region.
 29. Asystem according to claim 27, wherein the shape criteria is symmetry.30. A system according to claim 29, wherein the symmetry is tested byfinding a cross section through the region that result in the longestprofile in terms of distance from the first pixel encountered to thelast pixel encountered and comparing this distance to that obtained whenusing the perpendicular line to the longest axis.
 31. A system accordingto claim 23, wherein the processor controls the mechanical actuator tomove the mechanical actuator such that the sequence of points is dividedinto several sets, each set containing at least 3 points in a plane andat least one point out of the plane defined by the other points.
 32. Asystem according to claim 31, wherein the precise relative displacementsof these points is known by the processor using positional feedback fromthe mechanical actuator.
 33. A system according to claim 31, whereineach set is created by applying a different 6 degree of freedomtranslational and rotational offset to the standard untransformed setpoints to yield a new set of discrete points which have the samerelative positions.
 34. A method for calibrating a camera, the methodincluding: control a mechanical actuator to move at least one of anenergy source and a camera relative to the other through a plurality ofdiscrete points on a calibration target pattern; at each of the discretepoints, take a digital image with the camera; perform a lens distortioncharacterisation on each image; determine a focal length of the cameraincluding any lens connected to the camera; and determine an extrinsiccamera position for each image.
 35. A method according to claim 34,wherein the lens distortion characterisation is performed by: choosing adistortion correction model and determine an initial estimate ofparameters for this model to correct an observed distortion; choosing aline straightness metric, which measures and quantifies co-linear pointsalong a sampled line; and using the line straightness metric andnumerically refining the initial estimated parameters until the lines inthe distortion corrected image.
 36. A method according to claim 34,wherein the focal length is determined by: selecting an initial focallength; using algorithms in combination with the initial focal length,physical pixel sizes, undistorted image coordinates of the energy sourceat each point in the sequence, and the exact positions of the mechanicalactuator at each point in the sequence to determine the position of thecamera relative to each discrete point; determine how tightly clusteredthe camera positions are; and numerically refine the initial focallength until the determined discrete points are most tightly packed. 37.A method according to claim 34, wherein the extrinsic camera position isdetermined by: creating a bundle of geometry based vectors; creating abundle of image processing based vectors; choosing a metric to measurethe similarity of the two bundles of vectors; and refine an estimatedposition of the camera relative to the energy source to maximise thesimilarity of the bundles of vectors.
 38. A method according to claim37, wherein after the digital images have been captured, the methodfurther includes performing the following imaging processing steps:determine which regions of adjacent pixels in an image have an intensityabove a chosen threshold value; generate a list of such regions and thepixels which belong to each region together with the pixels' coordinatesand intensities; remove from this list any regions which have either toofew or too many constituent pixels as determined by characteristics ofthe camera, lens and energy source; remove from the list all regionsthat do not meet shape criteria; and determine a center of the largestremaining region.
 39. A method according to claim 38, wherein the centeris determined by fitting an ellipse to the region's pixel and using itscenter or by calculating the center of gravity of the pixels in theregion.
 40. A method according to claim 38, wherein the shape criteriais symmetry and wherein the symmetry is tested by finding a crosssection through the region that result in the longest profile in termsof distance from the first pixel encountered to the last pixelencountered and comparing this distance to that obtained when using theperpendicular line to the longest axis.
 41. A method according to claim34, wherein the mechanical actuator moves such that the sequence ofpoints is divided into several sets, each set containing at least 3points in a plane and at least one point out of the plane defined by theother points, and wherein the precise relative displacements of thesepoints is known using positional feedback from the mechanical actuator.42. A method according to claim 41, wherein each set is created byapplying a different 6 degree of freedom translational and rotationaloffset to the standard untransformed set points to yield a new set ofdiscrete points which have the same relative positions.